#ifndef PBAFACTORS_H
#define PBAFACTORS_H

#include "BaseTypes.hpp"
#include <ceres/ceres.h>

namespace Estimator
{
/*
*/
class CeresPBAFactor3Nodes : public InterfaceFactor<3>, public ceres::SizedCostFunction<3,  /*number of residuals*/
                                                            12, /*dimension of main anchor pose*/
                                                            12, /*dimension of associate anchor pose*/
                                                             5 /*dimension of parallax landmark*/>
                                                      
{
    typedef ::ceres::SizedCostFunction<3, 12, 12, 5> base_func;
    typedef InterfaceFactor<3> base_Inter;

  public:
    CeresPBAFactor3Nodes() = delete;

    CeresPBAFactor3Nodes(const Eigen::Vector3d& measurements, 
                         covariance_t& information) noexcept :
                         base_Inter(information, measurements)
    {}

    virtual ~CeresPBAFactor3Nodes() = default;

    virtual size_t parameterBlocks() const override {return 3; }

    virtual bool Evaluate(double const* const* parameters, double* residuals, 
                        double** jacobians) const override;       
};

class CeresPBAFactor4Nodes : public InterfaceFactor<3>, public ceres::SizedCostFunction<3,  /*number of residuals*/
                                                               12, /*dimension of main anchor pose*/
                                                               12, /*dimension of associate anchor pose*/
                                                               12, /*dimension of current pose*/
                                                               5 /*dimension of parallax landmark*/>
{
    typedef ::ceres::SizedCostFunction<3, 12, 12, 12, 5> base_func;
    typedef InterfaceFactor<3> base_Inter;

  public:
    CeresPBAFactor4Nodes() = delete;

    CeresPBAFactor4Nodes(const Eigen::Vector3d& measurements, 
                  covariance_t& information) noexcept :
                  base_Inter(information, measurements)
    {}

    virtual ~CeresPBAFactor4Nodes() = default;
  
    virtual size_t parameterBlocks() const override {return 4; }

    virtual bool Evaluate(double const* const* parameters, double* residuals, 
                        double** jacobians) const override;
   
};

class CeresTransOnlyFactor3Nodes : public InterfaceFactor<3>, public ceres::SizedCostFunction<3, /*number of residuals*/
                                                                                              3, /*dimension of main anchor pose*/
                                                                                              3 /*dimension of associate anchor pose*/>
{
    typedef ::ceres::SizedCostFunction<3, 3, 3> base_func;
    typedef InterfaceFactor<3> base_Inter;

  public:
    CeresTransOnlyFactor3Nodes() = delete;

    CeresTransOnlyFactor3Nodes(const Eigen::Vector3d &measurements,
                                covariance_t &information,
                                const Eigen::Matrix3d& mat,
                                const Eigen::Matrix3d& rot,
                                double sintheta) noexcept : 
                                base_Inter(information, measurements)/* , mCalibration(calibration) */,
                                                            mBzj(mat), mRoti(rot), msintheta(sintheta)
    {
    }

    virtual ~CeresTransOnlyFactor3Nodes() = default;

    virtual size_t parameterBlocks() const override { return 2; }

    virtual bool Evaluate(double const* const* parameters, double* residuals, 
                        double** jacobians) const override;       
    
    // Calibration mCalibration;
    const Eigen::Matrix3d mBzj;
    const Eigen::Matrix3d mRoti;
    double msintheta;
};


class CeresTransOnlyFactor4Nodes : public InterfaceFactor<3>, public ceres::SizedCostFunction<3, /*number of residuals*/
                                                                                              3, /*dimension of main anchor pose*/
                                                                                              3, /*dimension of associate anchor pose*/
                                                                                              3  /*dimension of current pose*/>
{
    typedef ::ceres::SizedCostFunction<3, 3, 3, 3 > base_func;
    typedef InterfaceFactor<3> base_Inter;

  public:
    CeresTransOnlyFactor4Nodes() = delete;

    CeresTransOnlyFactor4Nodes(const Eigen::Vector3d &measurements,
                                covariance_t &information,
                                const Eigen::Matrix3d& mat,
                                const Eigen::Matrix3d& rot,
                                double sintheta)noexcept : base_Inter(information, measurements)/* , mCalibration(calibration) */,
                                                           mBzj(mat), mRoti(rot), msintheta(sintheta)
    {}

    virtual ~CeresTransOnlyFactor4Nodes() = default;

    virtual size_t parameterBlocks() const override { return 3; }

    virtual bool Evaluate(double const* const* parameters, double* residuals, 
                        double** jacobians) const override;       
    
    // Calibration mCalibration;
    const Eigen::Matrix3d mBzj;
    const Eigen::Matrix3d mRoti;
    double msintheta;

};
}

#endif // PBAFACTORS_H